#include <iostream>

#include "ros/ros.h"
#include "actionlib/server/simple_action_server.h"
#include "demo/MoveAction.h"

void ActionCallback(const demo::MoveGoalConstPtr &goal, actionlib::SimpleActionServer<demo::MoveAction> *server) {
    if (goal.get() != nullptr) {
        uint32_t destination = goal.get()->destination;
        ROS_INFO("destination: %d", destination);
        ros::Rate rate(1);
        for (uint32_t distance = 0; distance < destination; ++distance) {
            demo::MoveFeedback feedback;
            feedback.distance = distance;
            server->publishFeedback(feedback);
            rate.sleep();
        }
        demo::MoveResult result;
        result.arrived = destination;
        server->setSucceeded(result);
    }
}

int main(int argc, char* argv[]) {
    std::string nodeName = "Server";
    ros::init(argc, argv, nodeName);
    ros::NodeHandle nodeHandle;
    std::string actionName = "move";
    actionlib::SimpleActionServer<demo::MoveAction> actionServer(nodeHandle, actionName, boost::bind(&ActionCallback, _1, &actionServer), false);
    actionServer.start();
    ros::spin();

    return EXIT_SUCCESS;
}